#include <stdio.h>
#include "vl53l0x_platform.h"

#include "SoftIIC.h"

#include "OpticalCar_hardware.h"


static SoftIIC_HardwareTypeDef g_iic_hw_1, g_iic_hw_2;
static VL53L0X_Dev_t g_dev1, g_dev2;


static void VL53L0X_Init(void) {
    // ===== Config IIC hardware =====
    g_iic_hw_1.RCC_SCL = RCC_SCL1;
    g_iic_hw_1.RCC_SDA = RCC_SDA1;
    g_iic_hw_1.port_SCL = PORT_IIC_SCL1;
    g_iic_hw_1.pin_SCL = PIN_IIC_SCL1;
    g_iic_hw_1.port_SDA = PORT_IIC_SDA1;
    g_iic_hw_1.pin_SDA = PIN_IIC_SDA1;
    
    SoftIIC_Init(&g_iic_hw_1);
    
    g_iic_hw_2.RCC_SCL = RCC_SCL2;
    g_iic_hw_2.RCC_SDA = RCC_SDA2;
    g_iic_hw_2.port_SCL = PORT_IIC_SCL2;
    g_iic_hw_2.pin_SCL = PIN_IIC_SCL2;
    g_iic_hw_2.port_SDA = PORT_IIC_SDA2;
    g_iic_hw_2.pin_SDA = PIN_IIC_SDA2;
    
    SoftIIC_Init(&g_iic_hw_2);
    
    g_dev1.iic_addr = 0x52;
    g_dev2.iic_addr = 0x52;
    g_dev1.iic_hw = &g_iic_hw_1;
    g_dev2.iic_hw = &g_iic_hw_2;
}


static void Test_WriteMulti(void) {
    VL53L0X_Error _err1, _err2;
    unsigned char _dat[3] = {0x01, 0x02, 0x03};

    _err1 = VL53L0X_WriteMulti(&g_dev1, 0xC0, _dat, sizeof(_dat));
    _err2 = VL53L0X_WriteMulti(&g_dev2, 0xC0, _dat, sizeof(_dat));
    
    printf("\x7F E2:%i, E1:%i", _err2, _err1);
}


static void Test_ReadMulti(void) {
    VL53L0X_Error _err1, _err2;
    unsigned char _dat1[3] = {0};
    unsigned char _dat2[3] = {0};
    
    _err1 = VL53L0X_ReadMulti(&g_dev1, 0xC0, _dat1, sizeof(_dat1));
    _err2 = VL53L0X_ReadMulti(&g_dev2, 0xC0, _dat2, sizeof(_dat2));
    
    // printf("\x7F");
    printf("%2X,%2X,%2X .......", _dat1[0], _dat1[1], _dat1[2]);
    printf("%2X,%2X,%2X ...... ", _dat2[0], _dat2[1], _dat2[2]);
    printf("E2:%i, E1:%i", _err2, _err1);
}


static void Test_WrByteWordDWord(void) {
    VL53L0X_Error _err1, _err2;
    
    _err1 = VL53L0X_WrByte(&g_dev1, 0xC0, 0x42);
    _err2 = VL53L0X_WrByte(&g_dev2, 0xC0, 0x43);

    printf("E2:%3i, E1:%3i, ", _err2, _err1);
    
    _err1 = VL53L0X_WrWord(&g_dev1, 0xC0, 0x4224);
    _err2 = VL53L0X_WrWord(&g_dev2, 0xC0, 0x4334);
    printf("E2:%3i, E1:%3i, ", _err2, _err1);
    
    _err1 = VL53L0X_WrDWord(&g_dev1, 0xC0, 0x12345678);
    _err2 = VL53L0X_WrDWord(&g_dev2, 0xC0, 0x12345678);
    printf("E2:%3i, E1:%3i, ", _err2, _err1);
    
}


static void Test_UpdateByte(void) {
    VL53L0X_Error _err1, _err2;
    
    _err1 = VL53L0X_UpdateByte(&g_dev1, 0xC0, 0x77, 0x11);
    _err2 = VL53L0X_UpdateByte(&g_dev2, 0xC0, 0x77, 0x11);
    printf("E2:%3i, E1:%3i, ", _err2, _err1);
}


static void Test_RdByteWordDWord(void) {
    VL53L0X_Error _err1, _err2;
    uint8_t _dat1[4], _dat2[4];
    uint16_t _w1, _w2;
    uint32_t _dw1, _dw2;
    
    _err1 = VL53L0X_RdByte(&g_dev1, 0xC0, _dat1);
    _err2 = VL53L0X_RdByte(&g_dev2, 0xC0, _dat2);
    printf("%3i %2X |", _err2, _dat2[0]);
    printf("%3i %2X  ", _err1, _dat1[0]);
    
    _err1 = VL53L0X_RdWord(&g_dev1, 0xC0, &_w1);
    _err2 = VL53L0X_RdWord(&g_dev2, 0xC0, &_w2);
    printf("%3i %4X", _err2, _w1);
    printf("%3i %4X", _err1, _w2);
    
    _err1 = VL53L0X_RdDWord(&g_dev1, 0xC0, &_dw1);
    _err2 = VL53L0X_RdDWord(&g_dev2, 0xC0, &_dw2);
    printf("%3i %8X....", _err2, _dw1);
    printf("%3i %8X....", _err1, _dw2);
}


static void VL53L0X_Test(void) {
    while (1) {
//        Test_WriteMulti();
        printf("\x7f");
        //Test_WrByteWordDWord();
        // Test_ReadMulti();
        //Test_UpdateByte();
        Test_RdByteWordDWord();
        
        // delay_us(200*1000);
    }
}
